
/***************************************************************************** \
 *                              ColUtilsOpenSG
\*****************************************************************************/

/*! @file
 *
 *  @brief
 *    Util-functions to help programm with OpenSG
 *
 *  @author Weiyu Yi, wyi@tu-clausthal.de
 *
 */

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <stdio.h>

#define COL_EXPORT

#include <col_import_export.h>
#include <ColUtilsOpenSG.h>

/// Collision detection namespace
namespace col {

/** Return the pointer to the multi-field of the points
 *
 * @warning
 *   Don't use the MFPnt3f pointer to modify the geometry!
 *   OpenSG will not notice the changes!
 *
 * @throw XCollision
 *   If any of the @c dynamic_cast's returns @c NULL.
 */
osg::MFPnt3f * getPoints( const osg::NodePtr node )
{
	const osg::GeometryPtr geo = getGeom( node );
	return getPoints( geo );
}


/** @overload
 */
osg::MFPnt3f* getPoints( const osg::GeometryPtr geo )
{
	const osg::GeoPositions3fPtr pos =
					osg::GeoPositions3fPtr::dcast(geo->getPositions());
	if ( pos.getCPtr() == NULL )
		throw XCollision("getPoints: dcast 2 failed!");
	return pos->getFieldPtr();
}


/** Return the GeoPositionsPtr of a node
 * @throw XCollision
 *   If any of the @c dynamic_cast's returns @c NULL.
 */
osg::GeoPositions3fPtr getPositions( const osg::NodePtr node )
{
	const osg::GeometryPtr geo = getGeom( node );
	const osg::GeoPositions3fPtr pos =
					osg::GeoPositions3fPtr::dcast( geo->getPositions() );
	if ( pos.getCPtr() == NULL )
		throw XCollision("getPoints: dcast failed!");
	return pos;
}

/** Return the pointer to the geometry core of the node
 * @throw XCollision
 *   If there is no geometry, i.e., if the dcast failed
 */
osg::GeometryPtr getGeom( const osg::NodePtr node )
{
	osg::GeometryPtr geo = osg::GeometryPtr::dcast(node->getCore());
	if ( geo.getCPtr() == NULL )
		throw XCollision("getGeom: dcast failed!\n"
						 "(node doesn't seem to have a geometry)");
	return geo;
}

/**  Dominant coord plane which v is "most orthogonal" to
 *
 * @param v		vector
 * @param x,y	indices of "most orthogonal" plane (out)
 *
 * Compute @a x and @a y, such that
 * \f$ \min(v_i) \leq v_x \; \wedge \; \min(v_i) \leq v_y \f$.
 **/
void dominantIndices( const Vec3f& v, unsigned int* x, unsigned int* y )
{
	if ( fabsf(v[0]) < fabsf(v[1]) )
		if ( fabsf(v[1]) < fabsf(v[2]) )
			*x = 0,  *y = 1;
		else
			*x = 0,  *y = 2;
	else
		if ( fabsf(v[0]) < fabsf(v[2]) )
			*x = 0,  *y = 1;
		else
			*x = 1,  *y = 2;
}

/** @overload
 *
 * @param v     Vector
 * @param x
 * @param y
 * @param z		index of smallest vector coord (out)
 *
 **/
void dominantIndices( const Vec3f& v,
					  unsigned int* x, unsigned int* y, unsigned int * z )
{
	if ( fabsf(v[0]) < fabsf(v[1]) )
		if ( fabsf(v[1]) < fabsf(v[2]) )
			*x = 0,  *y = 1, *z = 2;
		else
			*x = 0,  *y = 2, *z = 1;
	else
		if ( fabsf(v[0]) < fabsf(v[2]) )
			*x = 0,  *y = 1, *z = 2;
		else
			*x = 1,  *y = 2, *z = 0;
}

/**  Dominant coord axis which v is "most parallel" to
 *
 * @param v	vector
 * @return
 *   Index of maximum coordinate of @a v, such that
 *   \f$ v_x \geq \max(v_i) \f$.
 **/
unsigned int dominantIndex( const Vec3f& v )
{
	if ( fabsf(v[0]) < fabsf(v[1]) )
		if ( fabsf(v[1]) < fabsf(v[2]) )
			return 2;
		else
			return 1;
	else
		if ( fabsf(v[0]) < fabsf(v[2]) )
			return 2;
		else
			return 0;
}

/**  Average of an array of points
 *
 * @param points 	the array
 * @param npoints	number of points
 *
 * @return
 *   The average.
 **/
Pnt3f barycenter( const Pnt3f *points, const unsigned int npoints )
{
	Pnt3f c;
	for ( unsigned int i = 0; i < npoints; i ++ )
		for ( unsigned int j = 0; j < 3; j ++ )
			c[j] += points[i][j];
	for ( unsigned int j = 0; j < 3; j ++ )
		c[j] /= npoints;
	return c;
}

/** @overload
 */
Pnt3f barycenter( const vector<Pnt3f> &points )
{
	Pnt3f c;
	for ( unsigned int i = 0; i < points.size(); i ++ )
		for ( unsigned int j = 0; j < 3; j ++ )
			c[j] += points[i][j];
	for ( unsigned int j = 0; j < 3; j ++ )
		c[j] /= points.size();
	return c;
}

	
/**  Average of an array of indexed points
 *
 * @param points 				the array
 * @param index,nindices		array of indices into points
 *
 * @return
 *   The average over all points[ index[i] ], i = 0 .. nindices-1.
 *
 * @warning
 *   No range check on indices will done, i.e., indices pointing outside
 *   points[] will produce garbage or an FPE!
 **/
Pnt3f barycenter( const Pnt3f *points,
				  const unsigned int index[], const unsigned int nindices )
{
	Pnt3f c;
	for ( unsigned int i = 0; i < nindices; i ++ )
		for ( unsigned int j = 0; j < 3; j ++ )
			c[j] += points[ index[i] ][j];
	for ( unsigned int j = 0; j < 3; j ++ )
		c[j] /= nindices;
	return c;
}

/** @overload
 */
Pnt3f barycenter( const osg::MFPnt3f *points,
				  const unsigned int index[], const unsigned int nindices )
{
	Pnt3f c;
	for ( unsigned int i = 0; i < nindices; i ++ )
		for ( unsigned int j = 0; j < 3; j ++ )
			c[j] += (*points)[ index[i] ][j];
	for ( unsigned int j = 0; j < 3; j ++ )
		c[j] /= nindices;
	return c;
}

/** @overload
 */
Pnt3f barycenter( const vector<Pnt3f> &points, const TopoFace &face )
{
	Pnt3f c;
	for ( unsigned int i = 0; i < face.size(); i ++ )
		for ( unsigned int j = 0; j < 3; j ++ )
		{
				c[j] += points[ face[i] ] [j];
		}
	for ( unsigned int j = 0; j < 3; j ++ )
		c[j] /= face.size();
	return c;
}

	
/**  Sort vertices of a face such that they occur counter clockwise
 *
 * @param vertex		vertex array (in)
 * @param normal		normal of face (in)
 * @param face			vertex indices of the points of face (in/out)
 *
 * Sort all points in face so that they will be in counterclockwise order
 * when looked at from "outside"; "outside" is where the normal points at.
 *
 * @warning
 *   All @a face[i] should be valid indices into @a vertex!
 *
 * @pre
 *   All points of @a face should lie in one plane in 3D.
 *
 * @todo
 *   Use "Lamda Library" (Boost).
 *
 * @implementation
 *   The number of cosine evaluations is N*log(N); it could be reduced to N.
 **/
void sortVerticesCounterClockwise( const vector<Pnt3f> &vertex,
								   const Vec3f &normal,
								   TopoFace &face )
{
	unsigned int x, y;
	dominantIndices( normal, &x, &y );

	// shift center of mass to origin
	Pnt3f c = barycenter( vertex, face );

    //the function barycenter only returns Pnt3f
    //but we need a Vec3f in the loop 
    Vec3f temp;
    temp.setValues(c[0],c[1],c[2]);
	vector<Pnt3f> shifted_pnts( vertex.size() );
	for ( unsigned int i = 0; i < vertex.size(); i ++ )
    {
		//shifted_pnts[i] = vertex[i] - c;
        //only Pnt3f = Pnt3f - Vec3f is valid, Pnt3f = Pnt3f - Pnt3f is invalid
        shifted_pnts[i] = vertex[i] - temp;
    }

    // sort
	lessByAngle comp( x, y, shifted_pnts );
	sort( face.v.begin(), face.v.end(), comp );

	Vec3f e[3] = { Vec3f(1,0,0), Vec3f(0,1,0), Vec3f(0,0,1) };
	Vec3f cross = e[x] % e[y];
	if ( cross * normal < 0 )
		// swap order, because points have been projected on "back side"
		for ( unsigned int i = 0; i < face.size()/2; i ++ )
			swap( face[i], face[face.size()-1-i] );
}

	
/**  Create a polyhedron from simple vertex and face arrays
 *
 * @param vertex		vertex array (in)
 * @param face			faces array	(in, could get sorted)
 * @param gl_type		= GL_LINE_LOOP, GL_POLYGON, etc...
 * @param skip_redundant	skip faces with <3 vertices, if true
 * @param normals		normals array (can be NULL)
 *
 * @return
 *   The node.
 *
 * Create a polyhedron which is the object described by the planes
 * given by normals, nfaces, vertex, face, face_nv.
 * Actually, normals is only used for sorting the vertices of the faces.
 * If @a normals == NULL, then we assume that vertices in @a face[] are
 * already sorted in counter-clockwise order.
 * @a Face contains indices into vertex.
 *
 * There may be redundant faces, i.e., faces with face[i].size() = 0,1,2.
 * The geometry will have no material.
 *
 * No material is assigned.
 *
 * @warning
 *   If @a normals != NULL, then @a face @e will get sorted!
 *
 * @throw XColBug
 *   If there are no faces with any vertices.
 *
 * @pre
 *   - Planes are given by Ori*x - d = 0.
 *   - @a normals has @a face.size() many normals.
 *
 **/
osg::NodePtr geomFromPoints( const vector<Pnt3f> &vertex,
							 vector<TopoFace> &face,
							 int gl_type,
							 bool skip_redundant,
							 const Vec3f normals[] )
{
	// count # true faces
	unsigned int ntruefaces = 0;
	for ( unsigned int i = 0; i < face.size(); i ++ )
	{
		if ( face[i].size() > 0 )						// allow lines
			ntruefaces ++ ;
	}
	if ( ! ntruefaces )
		throw XColBug("geomFromPoints: no faces found");

	osg::NodePtr				node = osg::Node::create();
	osg::GeometryPtr			geom = osg::Geometry::create();
	osg::GeoPositions3f::PtrType pnts = osg::GeoPositions3f::create();
	osg::GeoIndicesUI32Ptr		index = osg::GeoIndicesUI32::create();	
	osg::GeoPLengthsUI32Ptr		lengths = osg::GeoPLengthsUI32::create();	
	osg::GeoPTypesUI8Ptr		type = osg::GeoPTypesUI8::create();	
	osg::GeoNormals3fPtr		norms;
	if ( normals )				norms = osg::GeoNormals3f::create();

	// reserve enough memory in MFields
	// TODO: do it with create(), when OSG supports it;
	// then we can use addValue() again
	pnts->getFieldPtr()->resize( vertex.size() );
	unsigned int indexlen = 0;
	for ( unsigned int i = 0; i < face.size(); i ++ )
		indexlen += face[i].size();
	index->getFieldPtr()->resize( indexlen );
	lengths->getFieldPtr()->resize( face.size() );
	type->getFieldPtr()->resize( face.size() );
	if ( normals )
		norms->getFieldPtr()->resize( face.size() );

	// node
	beginEditCP( node );
	node->setCore( geom );
	endEditCP( node );

	// geometry
	beginEditCP(pnts);
	for ( unsigned int i = 0; i < vertex.size(); i ++ )
		(*pnts->getFieldPtr())[i] = vertex[i];
	endEditCP(pnts);

	beginEditCP(index);
	beginEditCP(lengths);
	beginEditCP(type);
	if ( normals )
		beginEditCP(norms);

	unsigned int index_count = 0;
	for ( unsigned int i = 0; i < face.size(); i ++ )
	{
		if ( skip_redundant && face[i].size() < 3 )
			// skip redundant faces (can happen)
			continue;

		if ( normals && face[i].size() > 2 )
			sortVerticesCounterClockwise( vertex, normals[i], face[i] );

		// polygon
		for ( unsigned int j = 0; j < face[i].size(); j ++ )
		{
			if ( face[i][j] >= vertex.size() )
				throw XCollision("geomFromPoints: index face[%d][%d] = %d >= "
						 "num. vertices = %d", i,j, face[i][j], vertex.size() );

			(*index->getFieldPtr())[index_count] = face[i][j];
			index_count ++ ;
		}
		(*lengths->getFieldPtr())[i] = face[i].size();
		(*type->getFieldPtr())[i] = static_cast<osg::UInt8>( gl_type );
		if ( normals )
			(*norms->getFieldPtr())[i] = normals[i];
	}

	endEditCP(index);
	endEditCP(lengths);
	endEditCP(type);
	if ( normals )
		endEditCP(norms);

	beginEditCP(geom);
	geom->setPositions( pnts );
	geom->setIndices( index );
	geom->setLengths( lengths );
	geom->setTypes( type );
	if ( normals )
		geom->setNormals( norms );
	endEditCP(geom);

	return node;
}


/**  @overload
 *
 * @param vertex,nvertices		vertex array (in)
 * @param face,face_nv,nfaces	faces array	(in, could get sorted)
 * @param gl_type		= GL_LINE_LOOP, GL_POLYGON, etc...
 * @param skip_redundant	skip faces with <3 vertices, if true
 * @param normals		normals array (can be NULL)
 *
 * @a Face_nv[i] contains the number of vertices of @a face[i].
 * @a Nfaces contains the number of faces in @a face.
 * There may be redundant faces, i.e., faces with face_nv[i] = 0,1,2.
 *
 * @throw XColBug
 *  If a face has more than NumOri many vertices.
 *
 * @pre
 *   - @a normals has @a nfaces many normals.
 *   - @a face[i] @e must have @a Dop::NumOri many columns!
 *
 * @todo
 *   - immer noch wird die Variable NumOri gebraucht..
 **/
osg::NodePtr geomFromPoints( const Pnt3f vertex[], unsigned int nvertices,
                             unsigned int face[],
                             const unsigned int face_nv[],
                             unsigned int nfaces,
                             int gl_type,
                             bool skip_redundant,
                             const Vec3f normals[] )
{
	for ( unsigned int i = 0; i < nfaces; i ++ )
		if ( face_nv[i] > Dop::NumOri )
			throw XCollision("geomFromPoints: a face has more than "
						   "NumOri vertices");

	vector<TopoFace> face_vec( nfaces );

    int offset = 0;
    face_vec[0].set(face,face_nv[0]);
    for ( unsigned int i = 1; i < nfaces; i ++ )
    {    	
        offset = offset + face_nv[i-1];
		face_vec[i].set( face+offset, face_nv[i] );
    }
     
	vector<Pnt3f> vertex_vec( nvertices );
	for ( unsigned int i = 0; i < nvertices; i ++ )
		vertex_vec[i] = vertex[i];

	return geomFromPoints( vertex_vec, face_vec, gl_type,
						   skip_redundant, normals );
}

	
/**  Create a cube as OpenSG object
 *
 * @param radius		each side of the box will be 2*size long
 * @param gl_type		= GL_LINE_LOOP, GL_POLYGON, etc...

 * @return
 *   A NodePtr to the new cube.
 *
 * Creates a box with no material.
 *
 * @throw XCollision
 *   See geomFromPoints().
 *
 **/
osg::NodePtr makeCube( float radius, int gl_type )
{
	if ( gl_type != GL_POINTS && gl_type != GL_LINES &&
		 gl_type != GL_LINE_LOOP && gl_type != GL_POLYGON &&
		 gl_type != GL_QUADS )
		throw XCollision("makeCube: value of gl_type makes no sense");

	Pnt3f pnt[8] = { Pnt3f(-1,-1, 1),
					 Pnt3f( 1,-1, 1),
					 Pnt3f( 1, 1, 1),
					 Pnt3f(-1, 1, 1),
					 Pnt3f(-1,-1,-1),
					 Pnt3f( 1,-1,-1),
					 Pnt3f( 1, 1,-1),
					 Pnt3f(-1, 1,-1)
				   };
	/*unsigned int face[6][4] = { {0,1,2,3},
                                {1,5,6,2},
                                {2,6,7,3},
                                {0,3,7,4},
                                {0,4,5,1},
                                {4,7,6,5}
                              };
    */
    unsigned int face[6*4]={0,1,2,3,
                            1,5,6,2,
                            2,6,7,3,
                            0,3,7,4,
                            0,4,5,1,
                            4,7,6,5};
    
	unsigned int face_nv[6] = { 4, 4, 4, 4, 4, 4 };

	for ( unsigned int i = 0; i < 8; i ++ )
		pnt[i] *= radius;

	return geomFromPoints( pnt, 8, face, face_nv, 6, gl_type, false, NULL );
}
	
/**  Affine combination of two points
 *
 * @param pnt1,pnt2		points
 *
 * @return
 *   @a pnt1*c1 + @a pnt2*c2.
 *
 * @pre
 *   @a c1 + @a c2 = 1!
 **/
osg::Pnt3f lincomb( float c1, const osg::Pnt3f &pnt1, float c2, const osg::Pnt3f &pnt2 )
{
	osg::Pnt3f result;
	for ( unsigned int i = 0; i < 3; i ++ )
		result[i] = c1*pnt1[i] + c2*pnt2[i];
	return result;
}

	
/**  Calculate vertex normals for all geometries in a subtree
 *
 * @param node          root of subtree to be processed
 * @param creaseAngle   dihedral(?) angles larger than this won't be averaged (degrees)
 *
 **/

void calcVertexNormals( const osg::NodePtr node, const float creaseAngle /*  = 90 */ )
{
	osg::GeometryPtr geo = osg::GeometryPtr::dcast(node->getCore());
	if ( geo != osg::NullFC )
		osg::calcVertexNormals( geo, osg::deg2rad(creaseAngle) );

	for ( unsigned int i = 0; i < node->getNChildren(); i ++ )
		calcVertexNormals( node->getChild(i), creaseAngle );
}

}//namespace col
